#include "AStar.h"
AStar::AStar(AStarNode** gridData, int xSize, int ySize)
{
	this->gridData = gridData;
	this->xSize = xSize;
	this->ySize = ySize;
}
vector<AStarNode*> AStar::getPath(AStarNode* beginNode, AStarNode* targetNode)
{
	this->targetNode = targetNode;
	producePath(beginNode, targetNode);
	if (finish())
	{
		AStarNode* temp = targetNode;
		while (temp!= NULL)
		{
			path.push_back(temp);
			temp = temp->parent;
		}
		reverse(path.begin(), path.end());
		return path;
	}
	else
	{
		return vector<AStarNode*>();
	}
}
void AStar::producePath(AStarNode* beginNode, AStarNode* targetNode)
{
	//bool foundFlag = false;
	beginNode->parent = NULL;
	openList.push_back(beginNode);
	AStarNode* node = NULL;
	while (!openList.empty())
	{
		 node= getClosestNode();
		 //cout << "In:(" << node->position.y << "," << node->position.x << "),weight:"<<node->weight <<" G:"<<node->G<<" H:"<<node->H<< endl;
		 dealWithNode(node);
		 closeList.push_back(node);
		 openList.erase(find(openList.begin(), openList.end(), node));
		 if (finish())
		 {
			 //foundFlag = true;
			 break;
		 }
	}
}
bool AStar::finish()
{
	if (closeList.back() == targetNode)
		return true;
	return false;
}
AStarNode* AStar::getClosestNode()
{
	AStarNode* minNode = openList.front();
	for (auto t:openList)
	{
		if (t->F < minNode->F)
			minNode = t;
	}
	return minNode;
}
void AStar::dealWithNode(AStarNode* node)
{
	AStarNode* dealingNode=NULL;
	for (int i = 0; i < 4; i++)
	{
#pragma region select direction
		switch (i)
		{
		case 0:
			if (node->position.y-1>=0)
				dealingNode = &gridData[node->position.y-1][node->position.x];
			break;;
		case 1:
			if (node->position.x + 1 < xSize)
				dealingNode = &gridData[node->position.y][node->position.x+1];
			break;;
		case 2:
			if (node->position.y +1 <ySize)
				dealingNode = &gridData[node->position.y+1][node->position.x];
			break;
		case 3:
			if (node->position.x - 1 >=0)
				dealingNode = &gridData[node->position.y][node->position.x-1];
			break;
		default:
			break;
		}
#pragma endregion select direction
		if (dealingNode!=NULL&&dealingNode->weight != INFINITY&&!isInCloseList(dealingNode))
		{
			if (!isInOpenList(dealingNode))
			{
				dealingNode->G = node->G + 1 * node->weight;
				dealingNode->H =
					abs(targetNode->position.y - dealingNode->position.y) +
					abs(targetNode->position.x - dealingNode->position.x);
				dealingNode->F = dealingNode->G + dealingNode->H;
				openList.push_back(dealingNode);
				dealingNode->parent = node;
			}
			else
			{
				if (dealingNode->G > node->G + 1 * node->weight)
				{
					dealingNode->G = node->G + 1 * node->weight;
					dealingNode->F = dealingNode->G+dealingNode->H;
					dealingNode->parent = node;
				}
			}
		}
	}
}
bool AStar::isInOpenList(AStarNode* node)
{
	if (find(openList.begin(), openList.end(), node) != openList.end())
		return true;
	return false;
}
bool AStar::isInCloseList(AStarNode* node)
{
	if (find(closeList.begin(), closeList.end(), node) != closeList.end())
		return true;
	return false;
}